#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import os
import matplotlib.pyplot as plt
from pathlib import Path
from IPython import display
import glob
import sys
from PIL import Image
import matplotlib.animation as animation
sys.path.append('..')
from Dataloader.src import waymo_training_math_tools as math_tool
from Dataloader.src import waymo_training_plot_tools as plot_tool
from Dataloader.src.waymo_transfer_training_to_tf import *
from Dataloader.src.waymo_training_math_tools import *
import math
import numpy as np
from tqdm import tqdm 
data_path='/home/mdmp/ppp/dataset/uncompressed_scenario_training_training.tfrecord-00237-of-01000'
#data_path ='/home/mdmp/ppp/testing/uncompressed_scenario_testing_testing.tfrecord-00034-of-00150'
#waymo_dataset_file11='/home/mdmp/prpproject/WOMD_Dataset/training_dataset/uncompressed_scenario_training_training.tfrecord-00004-of-01000'

from Dataloader.src.waymo_scenario_metrics import *

waymo_dataset_list = []

waymo_dataset_list.append(data_path)
#waymo_dataset_list.append(waymo_dataset_file11)



generate_individual_images = True
generate_animation = True

def main():    
   dataloader = Dataloader(data_path)
   # # map_feature_list = dataloader.get_map_feature_list()
   # #d = dataloader.get_lane_center_id_dict()
   # #print(type(d[84]))
   
   egoid = 2162
   # # for key in d:
   # #    print(key)
   # #    print(d[key][0][0])
   lane_dist = dataloader.get_lane_center_id_dict()
   
   flag = dataloader.judge_whether_in_lane_byID(egoid,(23,45),1)
   print(flag)
   #plot_tool.visualization(data_path,dataloader,None, None,0,10,True,True)
   
   # onlanes = dataloader.get_agent_all_time_lane_list_byID(egoid)
   # print(onlanes)
   
   # a = dataloader.get_target_lane_points_byID(87)
   # print(a)  v 
   
   #plot_tool.trajectory_visualization_pos(data_path,dataloader,{},1,'/home/mdmp/ppp/waymo_trajectory_vis',10,90)
   # b = dataloader.get_front_agent_inTargetLane_byID(egoid,1)
   # print(b)
   # ppppp = dataloader.get_is_agent_on_lane_list_byID(tcp[0])
   # print(tcp[0])
   # print(ppppp)
   
   # top = dataloader.get_is_agent_on_lane_list_byID(tcp[0])
   # print(top)
   # # ttt,ppp = dataloader.get_lane_stop_sign_states_byID(238)
   # # print(ttt)
   # # print(ppp)
   # xx = dataloader.get_all_agents_lane_past_byID(tcp[0])
   # print(xx)
   # fo = dataloader.get_agent_front_lanes_byID(egoid)
   
   # print(fo)
   a,b = dataloader.get_agent_all_time_lane_list_byID(egoid)
   print(a)
   print(b)
   # plt.plot(c[0],c[1])
   # plt.savefig('/home/mdmp/ppp/waymo_trajectory_vis/sho11w.png')
   # ap = []
   # for i in range(90):
   #    aa,_ = dataloader.get_agent_distance_to_neibor_lane_byID(egoid,i)
   #    ap.append(aa)
   # print(ap)
   # length = dataloader.get_agent_length_list_byId(tcp[0])
   # print(length)
   # ddd = dataloader.get_all_agents_lane_past_byID()
   # print(ddd)
   
   # vvv = dataloader.get_all_time_cars_in_lane_upd_byID(278)
   # print(vvv)
   # egoid = dataloader.get_ego_vehicle_id()

   
   # # lll = dataloader.get_agent_all_time_lane_list_byID(egoid)
   # # print(lll)
   
   # # # #llll = dataloader.get_agent_in_lanes_byId(egoid)
   
   # # # # # print(len(llll))
   
   # # # # speed = dataloader.get_lane_speed_limit_byID(319)

   
   

   # lanes = dataloader.get_center_lane_id_list()
   # print(lanes)
   # leftlane,rightlane = dataloader.get_lane_boundary_byID(196)
   # print(leftlane)
   # print(rightlane)
   # aoo,_,_,_,_ = dataloader.get_around_cars_byID(egoid)
   # print(aoo)
   # ksks = dataloader.get_target_lane_points_byID(196)
   # print(ksks)
   # # for i in range(90):
   # #    right,left = dataloader.get_agent_distance_to_neibor_lane_byID(egoid,i)
   # #    print(right)
   # #    print(left)
   # # # # is_straight = dataloader.is_this_lane_crossroad_byID(317)
   
   # # # # print(is_straight)

   # track_heading = dataloader.get_agent_heading_list_byId(egoid)
   track_x = dataloader.get_agent_center_x_list_byId(egoid)
   track_y = dataloader.get_agent_center_y_list_byId(egoid)
   print(track_x)
   # v1 = dataloader.get_agent_v_x_list_byId(egoid)
   # v2 = dataloader.get_agent_v_y_list_byId(egoid)
   # print(track_heading)
   # print(v1)
   # print(v2)


   # # # front_lane_id = dataloader.is_this_lane_straight_byID(169)
   # # # print(front_lane_id)
   # # # # g,b = dataloader.get_agent_acc_and_acctheta_list_byId(egoid)
   # # # # print(g)
   # # # # print(b)
   # # # tc = dataloader.get_agent_center_x_list_byId(egoid)

   # ls,iid= dataloader.get_agent_all_time_lane_list_byID(egoid)

   # print(ls,iid)
   # # prppr,lss = dataloader.get_agent_all_time_lane_list_byID(egoid)
   
   # # print(prppr)
   
   # # print("zaizhe",lss)
   
   # # # # jj = dataloader.is_this_lane_straight_byID(194)
   # # # # print(jj)
   
   # # # bb = dataloader.find_independent_car_byID(egoid)
   # # # print(bb)
   # # # # print(bb)
   
   # # # # ll = dataloader.is_this_lane_crossroad_byID(172)
   # # # # zhege = dataloader.get_agent_all_time_lane_list(egoid)
   # # # # print(zhege)
   

   # # # #print(dir(dict[84]))
   
   # # # # print(dict[84].right_neighbors_id_dict)
   # # # # print(dict[84].right_boundary_segment_dict)
   # # # # iddiididid = dataloader.get_all_features_ids()
   # # # # print(iddiididid)s
   # # # # print(len(map_feature_list))
   # # # # for i in range(len(map_feature_list)):
   # # # #    print(type(map_feature_list[i]))
   # # # # print(map_feature_list[10].line_id)
   # ff,lf,lb,rf,rb = dataloader.get_around_cars_byID(egoid)
   # ans = []
   # for car in ff:
   #    lss = car[0]
   #    ans.append(lss)
   # print("front cars",ans)
   # print("all_around_cars",ff)
   # print("left front cars",lf)
   # print("left back cars",lb)
   # print("right front cars",rf)
   # print("right back cars",rb)
   
   # # # jj = dataloader.is_this_lane_straight_byID(194)
   # # # print(jj)
   
   # # bb = dataloader.get_distance_to_around_lane_byID(225)
   # # print(bb)
   
   # # ll = dataloader.is_this_lane_crossroad_byID(172)
   # # zhege = dataloader.get_agent_all_time_lane_list(egoid)
   # # print(zhege)
   

   # #print(dir(dict[84]))
   
   # # print(dict[84].right_neighbors_id_dict)
   # # print(dict[84].right_boundary_segment_dict)
   # # iddiididid = dataloader.get_all_features_ids()
   # # print(iddiididid)s
   # # print(len(map_feature_list))
   # # for i in range(len(map_feature_list)):
   # #    print(type(map_feature_list[i]))
   # # print(map_feature_list[10].line_id)
   # # pre_id_list = dataloader.get_all_car_id_list()
   # # print(pre_id_list)
   
   # # ooo = 0
   
   # # tx = track_x[89]
   # # ty = track_y[89]
   # # distance = 10000
   # # for key in lane_dist:
   # #    for i in range(0,len(lane_dist[key][0])):
   # #       tmp = math.sqrt((lane_dist[key][0][i]-tx)**2+(lane_dist[key][1][i]-ty)**2)
   # #       if tmp < distance:
   # #          distance = tmp
   # #          ooo = key
   # # print(ooo)
   # # print(lane[ooo].left_neighbors_id_dict[ooo])
   # # print(lane[ooo].right_neighbors_id_dict[ooo])
   # # print(lane[ooo].entry_lane_id_dict[ooo])
   # # print(lane[ooo].exit_lane_id_dict[ooo])

   # # lane_num200_points = lane_dist[200]
   # # lane_num196_points = lane_dist[196]

   # # print(lane_num196_points[0],len(lane_num200_points[0]))
      
   # # distance = math.sqrt((lane_num200_points[0][0]-lane_num196_points[0][0])**2 + (lane_num200_points[1][0]-lane_num196_points[1][0])**2)
   
   # # distance1 = math.sqrt((lane_num200_points[0][10]-lane_num196_points[0][10])**2 + (lane_num200_points[1][10]-lane_num196_points[1][10])**2)

   # # print(distance,distance1)

   
   
   # tra_list = [[5310.329460506434, 5310.614056385378, 5310.897036068613, 5311.18140368033, 5311.469787127456, 5311.764625740907, 5312.068172558737, 5312.38249736633, 5312.709490065291, 5313.050864174859, 5313.408160468998, 5313.782750719256, 5314.175841545633, 5314.588478362744, 5315.021549405105, 5315.475789837333, 5315.951785923445, 5316.449979254647, 5316.9706710390365, 5317.513984733087, 5318.079603357181, 5318.66695003528, 5319.275331843553, 5319.903943559205, 5320.5518706190815, 5321.218091490194, 5321.901465570715],
   # [9347.621315959312, 9348.873612961692, 9350.125696442472, 9351.377792162515, 9352.629359437042, 9353.879939605702, 9355.129135977348, 9356.376606705182, 9357.622058719055, 9358.865242045222, 9360.105944592528, 9361.343987246075, 9362.579219426298, 9363.811514941252, 9365.040768289542, 9366.266891245057, 9367.489809835062, 9368.709461609844, 9369.925793259516, 9371.138828954277, 9372.348969526727, 9373.556774233472, 9374.762864733782, 9375.967922035721, 9377.172682481449, 9378.377931681873, 9379.58436746118]]
   # plot_tool.visualize_on_step(data_path,dataloader,tra_list,0)
   # plot_tool.visualize_on_step(data_path,dataloader,tra_list,1)
   
   # dictt = {1024:[[0.5,0.5],[[1,2],[3,4]],[[5,6],[7,8]]],1032:[[0.5,0.5],[[1,2],[3,4]],[[5,6],[7,8]]]}
   # dictt = {egoid:[[1],[0,1,2,[[3,4]],[[4,5]]],[0,1,2,[[3,4]],[[4,5]]]]}
   # dictt = {}
   # plot_tool.trajectory_visualization(data_path,dataloader,dictt,1,'/home/mdmp/ppp/waymo_trajectory_vis',10,90,True)
   # posdict = {egoid:[[21,23,4,5,6,6,6],[133,6,6,5,7,7,7]]}
   # plot_tool.trajectory_visualization_pos(data_path,dataloader,posdict,1,'/home/mdmp/ppp/waymo_trajectory_vis',10,90,True)
   
   # xx,yy,zz = dataloader.ground_truth_transfer()
   # print(xx.shape)
   # print(yy.shape)
   # print(type(zz))
   metrics = GetMetrics(dataloader)
   
   
if __name__ == "__main__":
    main()
    
